﻿
// 2024/2/26: 首个版本

#ifndef WALKER_CONTROL_V1_H
#define WALKER_CONTROL_V1_H

#include "spadas.h"

namespace walker_control_v1
{
	using namespace spadas;

	struct WalkerControl
	{
		FullTimestamp timestamp; // 时间戳

		Bool requestControl; // Weather to request control / 是否请求控制
		Double heading; // [deg] Heading of moving, 0 as north, CCW is positive / 行进朝向，0为正北，逆时针为正
		Double speed; // [KPH] Moving speed, 0 is static / 移动速度，0为静止

		WalkerControl()
		{
			requestControl = FALSE;
			heading = 0;
			speed = 0;
		}

		static String getProtocolName(UInt channel)
		{
			if (channel == UINF) return "walker-control-v1";
			else return (String)"walker-control-v1@" + channel;
		}

		static Bool supportInterpolation()
		{
			return FALSE;
		}

		SessionGeneralSample toGeneralSample()
		{
			SessionGeneralSample sample;
			sample.timestamp = timestamp;
			sample.values = Array<GeneralElement>(3);
			GeneralElement *data = sample.values.data();

			data[0] = requestControl ? 1 : 0;
			data[1] = heading;
			data[2] = speed;

			sample.significantCount = sample.values.size();
			return sample;
		}

		Bool fromGeneralSample(String protocol, SessionGeneralSample sample)
		{
			timestamp = sample.timestamp;

			if (protocol == "walker-control-v1")
			{
				if (sample.values.size() != 3) return FALSE;

				requestControl = sample.values[0].valid && sample.values[0].value == 1;
				heading = sample.values[1].value;
				speed = sample.values[2].value;

				return TRUE;
			}

			return FALSE;
		}

		static WalkerControl interpolate(WalkerControl& s1, Double w1, WalkerControl& s2, Double w2, FullTimestamp timestamp)
		{
			return WalkerControl();
		}
	};
}

#endif
